﻿"""
@file example.py

© [2023] LimX Dynamics Technology Co., Ltd. All rights reserved.
"""

import sys
import time
from functools import partial
import limxsdk.robot.Rate as Rate
import limxsdk.robot.Robot  as Robot
import limxsdk.robot.RobotType as RobotType
import limxsdk.datatypes as datatypes
import configparser
from enum import Enum
import threading
# for test head action

# from utils_head_motor import Head
# head = Head()


class GameAxis(Enum):
    """
    定义一个枚举类型，表示游戏手柄的摇杆

    Logit F710 Direct：left_horizon0 left_vertival1 right_horizon2 right_vertival3
    
    GameSir G4S：left_horizon2 left_vertival3 right_horizon0 right_vertival1

    Limx Gamepad：left_horizon2 left_vertival3 right_horizon0 right_vertival1
    """
    left_horizon = 0  # 左摇杆水平轴
    left_vertival = 1  # 左摇杆垂直轴
    right_horizon = 2  # 右摇杆水平轴
    right_vertival = 3  # 右摇杆垂直轴

class GameButton(Enum):  
    
    """
    定义一个枚举类型，表示游戏手柄的按钮

    Logit F710 Direct：X0 A1 B2 Y3 LB4 RB5 LT6 RT7 BACK8 START9
    
    GameSir G4S：X3 A0 B1 Y4 LB6 RB7 LT8 RT9

    Limx Gamepad：A(X)0 B(〇)1 X(口)2 Y(△)3 L14 R25 L36 R47 SELECT8 START9 UP12 DOWN13 LEFT14 RIGHT15 MENU16 BACK17
    """
    A = 0
    B = 1
    X = 2
    Y = 3
    L1 = 4
    L2 = 6
    R1 = 7
    R2 = 5
    SELECT = 8
    START = 9
    UP = 12
    DOWN = 13
    LEFT = 14
    RIGHT = 15
    MENU = 16
    BACK = 17

class Debouncer:
    def __init__(self, debounce_time):
        self.debounce_time = debounce_time
        self.last_press_time = {}

    def is_pressed(self, button):
        current_time = time.time()
        if button not in self.last_press_time or current_time - self.last_press_time[button] > self.debounce_time:
            self.last_press_time[button] = current_time
            return True
        return False
    
def load_config(ini_path):
    """
    加载配置文件并返回配置对象。

    参数:
    ini_path (str): 配置文件的路径。

    返回:
    configparser.ConfigParser: 配置对象。
    """           
    config = configparser.ConfigParser()
    config.read(ini_path)
    return config

def map_button_to_action(buttons, ini_path='config.ini'):
    """
    根据手柄按钮组合映射到相应的动作。

    参数:
    button1 (int): 第一个按钮的索引。
    button2 (int): 第二个按钮的索引。
    ini_path (str): 配置文件的路径，默认为 'config.ini'。

    返回:
    str: 对应的动作，如果未找到则返回 None。
    """
    
    button1 = buttons[0]
    button2 = buttons[1] if len(buttons) >= 2 else None
    # 加载配置
    config = load_config(ini_path)

    # 获取按钮名称
    button1_name = GameButton(button1).name
    
    action_key = ""
    if button2 is None:
        # 查找对应的组合动作
        action_key = f"{button1_name}"
    else:
        button2_name = GameButton(button2).name
        action_key = f"{button1_name}_{button2_name}"

        # 如果没有这个组合键，尝试反转组合键 (比如 X_A 和 A_X 都可以)
        if action_key not in config['gamepad']:
            action_key = f"{button2_name}_{button1_name}"

    print(f"Pressed {action_key}")
    # 获取动作
    action = config['gamepad'].get(action_key, None)
    # print(f"Assigned action: {action}")
    return action

class LimxReceiver:
    def __init__(self):
        self.debouncer = Debouncer(1)  # Set debounce time to 0.1 seconds
    
    def play_action(self, action):
        # 根据传入的action参数，打印相应的字符
        print(f"action:{action}")
        # time.sleep(10)
        # print(f"action print 2:{action}")

    # Callback function for receiving sensor joy data
    def sensorJoyCallback(self, sensor_joy: datatypes.SensorJoy, config_path='config.ini'):
        # print("\n------\nsensor_joy:" + \
        #       "\n  stamp: " + str(sensor_joy.stamp) + \
        #       "\n  axes: " + str(sensor_joy.axes) + \
        #       "\n  buttons: " + str(sensor_joy.buttons))
        pressed_buttons = [i for i, x in enumerate(sensor_joy.buttons) if x == 1]
        filtered_buttons = [button for button in pressed_buttons if self.debouncer.is_pressed(button)]
        action = map_button_to_action(buttons=filtered_buttons, ini_path= config_path)
        t1 = threading.Thread(target=self.play_action, args=(action,))
        t1.start()
        # self.play_action(action)

def initialize_robot(robot_ip):
    """
    初始化机器人
    """
    robot = Robot(RobotType.PointFoot)
    assert robot.init(robot_ip), "Failed to initialize robot"
    
    # print("Set light effect to STATIC_RED: " + str(robot.setRobotLightEffect(datatypes.LightEffect.STATIC_RED)))
    # time.sleep(5)
    # print("Set light effect to FAST_FLASH_YELLOW: " + str(robot.setRobotLightEffect(datatypes.LightEffect.FAST_FLASH_YELLOW)))
    # time.sleep(5)
    return robot

def main():
    robot_ip = sys.argv[1] if len(sys.argv) > 1 else "10.192.1.2"
    robot = initialize_robot(robot_ip)
    receiver = LimxReceiver()
    sensorJoyCallback = partial(receiver.sensorJoyCallback)
    robot.subscribeSensorJoy(sensorJoyCallback)
    while True:
        pass

if __name__ == '__main__':
    main()
